//
// File: rfnn_fcn_v2.cpp
//
// MATLAB Coder version            : 5.4
// C/C++ source code generated on  : 15-Dec-2024 09:42:07
//

// Include Files
#include "rfnn_fcn_v2.h"
#include <cmath>

// Function Definitions
//
// Arguments    : const double a[32]
//                double b
//                double dg
//                double sig
//                double dob
//                double sio
//                double *vR
//                double *vL
//                double Fnk[16]
//                double Outputs[2]
// Return Type  : void
//
void rfnn_fcn_v2(const double a[32], double b, double dg, double sig,
                 double dob, double sio, double *vR, double *vL, double Fnk[16],
                 double Outputs[2])
{
  static const double c[8]{0.0,
                           10.0,
                           0.0,
                           4.0,
                           -1.5707963267948966,
                           1.5707963267948966,
                           -1.5707963267948966,
                           1.5707963267948966};
  static const signed char iv[4]{5, 1, 1, 1};
  double W[32];
  double MFs[8];
  double S[4];
  double firing_total;
  double v;
  int b_a;
  int o;
  // 最小转向半径
  S[0] = dg;
  S[1] = dob;
  S[2] = sig;
  S[3] = sio;
  // 神经网络部分计算输出
  firing_total = ((dg + dob) + sig) + sio;
  for (b_a = 0; b_a < 16; b_a++) {
    o = b_a << 1;
    W[o] = a[b_a] * firing_total;
    W[o + 1] = a[b_a + 16] * firing_total;
  }
  // coder.inline("never");
  //  参数初始化
  //  输入层节点数
  //  每个输入的隶属度函数数
  //  模糊规则数
  //  输出层节点数
  //  初始化静态变量
  //      persistent pre_firingStrengths;
  //      if isempty(pre_firingStrengths)
  //              pre_firingStrengths =zeros(1,numRules);
  //      end
  // 训练均值和标准差
  //      sigma=[4.3,1.6,1.3,1.3];%v,w
  // v,w
  //  隶属度层
  for (int i{0}; i < 4; i++) {
    int a_tmp;
    b_a = iv[i];
    //  定义高斯隶属度函数的参数
    // 均值
    // v标准差
    a_tmp = i << 1;
    firing_total = S[i];
    v = firing_total - c[a_tmp];
    o = b_a * b_a;
    MFs[a_tmp] =
        std::fmax(std::exp(-(v * v) / static_cast<double>(o)), 1.0E-10);
    //  定义高斯隶属度函数的参数
    // 均值
    // v标准差
    v = firing_total - c[a_tmp + 1];
    MFs[a_tmp + 1] =
        std::fmax(std::exp(-(v * v) / static_cast<double>(o)), 1.0E-10);
  }
  //  模糊规则层
  for (b_a = 0; b_a < 16; b_a++) {
    //  计算每条规则的激活程度（AND操作）
    switch (b_a + 1) {
    case 1:
      //                  firingStrengths(r)=min([MFs(1,1),MFs(1,2),MFs(1,3),MFs(1,4)]);%near
      //                  near right right
      Fnk[b_a] = MFs[0] * MFs[2] * MFs[4] * MFs[6];
      break;
    case 2:
      //                  firingStrengths(r)=min([MFs(1,1),MFs(1,2),MFs(1,3),MFs(2,4)]);%near
      //                  near right left
      Fnk[b_a] = MFs[0] * MFs[2] * MFs[4] * MFs[7];
      break;
    case 3:
      //                  firingStrengths(r)=min([MFs(1,1),MFs(1,2),MFs(2,3),MFs(1,4)]);%near
      //                  near left right
      Fnk[b_a] = MFs[0] * MFs[2] * MFs[5] * MFs[6];
      break;
    case 4:
      //                  firingStrengths(r)=min([MFs(1,1),MFs(1,2),MFs(2,3),MFs(2,4)]);%near
      //                  near left left
      Fnk[b_a] = MFs[0] * MFs[2] * MFs[5] * MFs[7];
      break;
    case 5:
      //                  firingStrengths(r)=min([MFs(1,1),MFs(2,2),MFs(1,3),MFs(1,4)]);%near
      //                  far right right
      Fnk[b_a] = MFs[0] * MFs[3] * MFs[4] * MFs[6];
      break;
    case 6:
      //                  firingStrengths(r)=min([MFs(1,1),MFs(2,2),MFs(1,3),MFs(2,4)]);%near
      //                  far right left
      Fnk[b_a] = MFs[0] * MFs[3] * MFs[4] * MFs[7];
      break;
    case 7:
      //                  firingStrengths(r)=min([MFs(1,1),MFs(2,2),MFs(2,3),MFs(1,4)]);%near
      //                  far left right
      Fnk[b_a] = MFs[0] * MFs[3] * MFs[5] * MFs[6];
      break;
    case 8:
      //                  firingStrengths(r)=min([MFs(1,1),MFs(2,2),MFs(2,3),MFs(2,4)]);%near
      //                  far left left
      Fnk[b_a] = MFs[0] * MFs[3] * MFs[5] * MFs[7];
      break;
    case 9:
      //                  firingStrengths(r)=min([MFs(2,1),MFs(1,2),MFs(1,3),MFs(1,4)]);%far
      //                  near right right
      Fnk[b_a] = MFs[1] * MFs[2] * MFs[4] * MFs[6];
      break;
    case 10:
      //                  firingStrengths(r)=min([MFs(2,1),MFs(1,2),MFs(1,3),MFs(2,4)]);%far
      //                  near right left
      Fnk[b_a] = MFs[1] * MFs[2] * MFs[4] * MFs[7];
      break;
    case 11:
      //                  firingStrengths(r)=min([MFs(2,1),MFs(1,2),MFs(2,3),MFs(1,4)]);%far
      //                  near left right
      Fnk[b_a] = MFs[1] * MFs[2] * MFs[5] * MFs[6];
      break;
    case 12:
      //                  firingStrengths(r)=min([MFs(2,1),MFs(1,2),MFs(2,3),MFs(2,4)]);%far
      //                  near left left
      Fnk[b_a] = MFs[1] * MFs[2] * MFs[5] * MFs[7];
      break;
    case 13:
      //                  firingStrengths(r)=min([MFs(2,1),MFs(2,2),MFs(1,3),MFs(1,4)]);%far
      //                  far right right
      Fnk[b_a] = MFs[1] * MFs[3] * MFs[4] * MFs[6];
      break;
    case 14:
      //                  firingStrengths(r)=min([MFs(2,1),MFs(2,2),MFs(1,3),MFs(2,4)]);%far
      //                  far right left
      Fnk[b_a] = MFs[1] * MFs[3] * MFs[4] * MFs[7];
      break;
    case 15:
      //                  firingStrengths(r)=min([MFs(2,1),MFs(2,2),MFs(2,3),MFs(1,4)]);%far
      //                  far left right
      Fnk[b_a] = MFs[1] * MFs[3] * MFs[5] * MFs[6];
      break;
    default:
      //                  firingStrengths(r)=min([MFs(2,1),MFs(2,2),MFs(2,3),MFs(2,4)]);%far
      //                  far left left
      Fnk[b_a] = MFs[1] * MFs[3] * MFs[5] * MFs[7];
      break;
    }
  }
  //      firingStrengths=round(firingStrengths,3);
  firing_total = Fnk[0];
  for (b_a = 0; b_a < 15; b_a++) {
    firing_total += Fnk[b_a + 1];
  }
  for (b_a = 0; b_a < 16; b_a++) {
    Fnk[b_a] /= firing_total;
  }
  //      if(pre_firingStrengths~=zeros(1,numRules))
  //          now_firingStrengths=0.8*now_firingStrengths+0.2*pre_firingStrengths;
  //      end
  //      now_firingStrengths=firingStrengths;
  //      time_delay=zeros(1,numRules);
  //      now_firingStrengths=(ones(1,numRules)-time_delay).*firingStrengths+time_delay.*pre_firingStrengths;
  // 后件层
  //      [rulesOn,i]=max(now_firingStrengths);
  //      Outputs=rulesOn*W(:,i);
  for (o = 0; o < 2; o++) {
    firing_total = 0.0;
    for (b_a = 0; b_a < 16; b_a++) {
      //  线性组合
      firing_total += Fnk[b_a] * W[o + (b_a << 1)];
    }
    Outputs[o] = firing_total;
  }
  *vR = Outputs[0] / (2.4 * std::abs(Outputs[0]) + 4.0);
  *vL = Outputs[1] / (2.4 * std::abs(Outputs[1]) + 4.0);
  v = (*vR + *vL) / 2.0;
  if (std::abs(v / ((*vR - *vL) / (2.0 * b))) < 0.165) {
    if (*vR > *vL) {
      firing_total = b * (v / 0.165);
      *vR = v + firing_total;
      *vL = v - firing_total;
    } else {
      firing_total = b * (-v / 0.165);
      *vR = v + firing_total;
      *vL = v - firing_total;
    }
  }
}

//
// File trailer for rfnn_fcn_v2.cpp
//
// [EOF]
//
